
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       fms.c
  * @author     baiyang
  * @date       2023-6-29
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "fms.h"
#include "logger.h"

#include <assert.h>

#include <rtthread.h>
#include <rtdevice.h>
#include <rthw.h>

#include <uITC/uITC.h>
#include <uITC/uITC_msg.h>

#include <notify/notify.h>
#include <common/microbee.h>
#include <perf/perf_counter.h>
#include <srv_channel/srv_channel.h>
#include <board_config/board_config.h>
#include <mb_navigation/mb_l1_control.h>
#include <srvrly_events/srvrly_events.h>
#include <battery_monitor/battery_monitor.h>
#include <sensor_airspeed/sensor_airspeed.h>
/*-----------------------------------macro------------------------------------*/
#define FMS_EVENT_LOOP        (1<<0)
/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/

/*----------------------------------variable----------------------------------*/
#if defined(HAL_NO_CCMRAM)
static char thread_fms_stack[1024*8];
#else
static char thread_fms_stack[1024*8] RT_SECTION(".ccmram");
#endif

struct rt_thread thread_fms_handle;

static struct rt_timer fms_timer;
static struct rt_event fms_event;

// list of priorities, highest priority first
static const int8_t _failsafe_priorities[] = {Failsafe_Action_Terminate,
                                              Failsafe_Action_Parachute,
#if HAL_QUADPLANE_ENABLED
                                              Failsafe_Action_QLand,
#endif
                                              Failsafe_Action_Land,
                                              Failsafe_Action_RTL,
                                              Failsafe_Action_None,
                                              -1 // the priority list must end with a sentinel of -1
                                             };

perf_counter_t _fms_elapsed_perf;
perf_counter_t _fms_interval_perf;

struct fms_handle fms;
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
void fms_ctor()
{
    fms_assign_param();

    _fms_elapsed_perf = perf_alloc(PC_ELAPSED, "fms: ET");
    _fms_interval_perf = perf_alloc(PC_INTERVAL, "fms: INR");

    fms_ahrs_init();
    mb_arming_init();

    fms_init_rc_in();
    fms_init_rc_out_main();
    fms_init_rc_out_aux();

    fms.ahrs = get_ahrs_view();

    minit_ctor(&fms.mode_initializing);

    fms.flight_modes = &fms.g.flight_modes[0];
    fms.num_flight_modes = 6;

    fms.control_mode = (mode_base_t)&fms.mode_initializing;
    fms.previous_mode = (mode_base_t)&fms.mode_initializing;
    fms.control_mode_reason = MODE_REASON_UNKNOWN;
    fms.previous_mode_reason = MODE_REASON_UNKNOWN;

    fms.G_Dt = PERIOD_MS_250HZ * 0.001f;
    fms.FAST_Dt = PERIOD_MS_500HZ * 0.001f;

    rctrl_ctor(&fms.roll_controller, &fms.aparm, PERIOD_MS_250HZ * 0.001f);
    pctrl_ctor(&fms.pitch_controller, &fms.aparm, PERIOD_MS_250HZ * 0.001f);
    yctrl_ctor(&fms.yaw_controller, &fms.aparm, PERIOD_MS_250HZ * 0.001f);
    sctrl_ctor(&fms.steer_controller);

    static struct mb_l1_control l1_control;
    l1_control_ctor(&l1_control);

    fms.nav_controller = (navctrl_t)&l1_control;

    fms.ground_start_count = 5;
    fms.new_airspeed_cm = -1;
    fms.surface_speed_scaler = 1.0f;

    fms.steer_state.hold_course_cd = -1;

    fms.guided_state.target_airspeed_cm = -1;  // don't default to zero here, as zero is a valid speed.
    fms.guided_state.target_alt = -1;   // don't default to zero here, as zero is a valid alt.
    fms.guided_state.last_target_alt = 0;
    fms.guided_state.target_alt_time_ms = 0;
    fms.guided_state.target_alt_frame = 0;
    fms.guided_state.target_heading = -4; // don't default to zero or -1 here, as both are valid headings in radians

    fms.auto_state.next_turn_angle = 90;

    fms.flight_stage = FWFS_NORMAL;
    fms.aerodynamic_load_factor = 1.0f;
    fms.crow_mode = CM_NORMAL;

    fms.home = &fms.ahrs->home_loc;

    mmanual_ctor(&fms.mode_manual);
    mfbwa_ctor(&fms.mode_fbwa);
    mstabilize_ctor(&fms.mode_stabilize);
    macro_ctor(&fms.mode_acro);

    battmonitor_ctor(MASK_LOG_CURRENT, fms_handle_battery_failsafe, _failsafe_priorities);
    battmonitor_init();

    mission_init(start_command_callback, verify_command_callback, exit_mission_callback);

    fms_gcs_init();

    fms_actuator_armed_notify(false);

    sensor_as_set_fixedwing_parameters(&fms.aparm);

    aplog_set_vehicle_startup_writer(aplog_write_vehicle_startup_messages);
}

/**
  * @brief       1Hz循环
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
static inline void fms_one_hz_loop()
{
    fms_set_control_channels();
    srv_channels_enable_aux_servos();

    mb_arming_update();

    // update notify flags
    notify_flags.pre_arm_check = mb_arming_pre_arm_checks(false);
    notify_flags.pre_arm_gps_check = true;
    notify_flags.armed = mb_arming_is_armed() || mb_arming_required() == ARMING_REQUIRED_NO;

    uitc_sensor_gps vehicle_gps_position;
    itc_copy_from_hub(ITC_ID(sensor_gps),  &vehicle_gps_position);

    // update home position if NOT armed and gps position has
    // changed. Update every 5s at most
    if (!mb_arming_is_armed() &&
        vehicle_gps_position.last_message_time_ms - fms.last_home_update_ms > 5000 &&
        vehicle_gps_position.fix_type >= uITC_GPS_OK_FIX_3D) {
            fms.last_home_update_ms = vehicle_gps_position.last_message_time_ms;
            fms_update_home();
#if 0
            // reset the landing altitude correction
            landing.alt_offset = 0;
#endif
    }
}

/**
  * @brief       2Hz循环
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
static inline void fms_two_hz_loop()
{

}

/**
  * @brief       5Hz循环
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
static inline void fms_fives_hz_loop()
{
    fms_publish_vehicle_hearbeat();
    fms_update_is_flying_5Hz();
}

/**
  * @brief       10Hz循环
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
static inline void fms_ten_hz_loop()
{
    srv_hal_safety_update();
    battmonitor_read();
    rcs_read_mode_switch();
    rcs_read_aux_all();
    fms_publish_vehicle_vfr_hud();
}

/**
  * @brief       20Hz循环
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
static inline void fms_twenty_hz_loop()
{

}

/**
  * @brief       50Hz循环
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
static inline void fms_fifty_hz_loop()
{
    notify_update();
    fms_read_radio();
    srvrly_update_events();
}

/**
  * @brief       100Hz循环
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
static inline void fms_one_hundred_hz_loop()
{

}

/**
  * @brief       200Hz循环
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
static inline void fms_two_hundred_hz_loop()
{
    fms_gcs_update();
}

/**
  * @brief       250Hz循环
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
static inline void fms_two_hundred_and_fifty_hz_loop()
{
    fms_update_control_mode();
    fms_stabilize();
}

/**
  * @brief       500Hz循环
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        用于更新需要最先更新的数据
  */
static inline void fms_fast_loop()
{
    fms_update_ahrs();
    fms_set_servos();
}

/**
  * @brief       
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
static inline void fms_loop()
{
    perf_begin(_fms_elapsed_perf);
    perf_count(_fms_interval_perf);

    uint32_t NowMs = time_millis();

    TIMETAG_CHECK_EXECUTE2(fast,PERIOD_MS_500HZ,NowMs,fms_fast_loop();)

    TIMETAG_CHECK_EXECUTE2(100Hz,PERIOD_MS_100HZ,NowMs,fms_one_hundred_hz_loop();)
    TIMETAG_CHECK_EXECUTE2(250Hz,PERIOD_MS_250HZ,NowMs,fms_two_hundred_and_fifty_hz_loop();)
    TIMETAG_CHECK_EXECUTE2(200Hz,PERIOD_MS_200HZ,NowMs,fms_two_hundred_hz_loop();)

    TIMETAG_CHECK_EXECUTE2(50Hz,PERIOD_MS_50HZ,NowMs,fms_fifty_hz_loop();)
    TIMETAG_CHECK_EXECUTE2(25Hz,PERIOD_MS_20HZ,NowMs,fms_twenty_hz_loop();)
    TIMETAG_CHECK_EXECUTE2(10Hz,PERIOD_MS_10HZ,NowMs,fms_ten_hz_loop();)
    TIMETAG_CHECK_EXECUTE2(5Hz,PERIOD_MS_5HZ,NowMs,fms_fives_hz_loop();)
    TIMETAG_CHECK_EXECUTE2(2Hz,PERIOD_MS_2HZ,NowMs,fms_two_hz_loop();)
    TIMETAG_CHECK_EXECUTE2(1Hz,PERIOD_MS_1HZ,NowMs,fms_one_hz_loop();)

    perf_end(_fms_elapsed_perf);
}

/**
  * @brief       定时器回调函数，发送定时器事件
  * @param[in]   parameter  
  * @param[out]  
  * @retval      
  * @note        
  */
static void fms_time_update(void* parameter)
{
    rt_event_send(&fms_event, FMS_EVENT_LOOP);
}

/**
  * @brief       飞行任务线程入口函数
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
static void fms_entry(void *parameter)
{
    rt_err_t res;
    rt_uint32_t recv_set = 0;
    rt_uint32_t wait_set = FMS_EVENT_LOOP;

    /* create event */
    res = rt_event_init(&fms_event, "fms", RT_IPC_FLAG_FIFO);
    
    /* register timer event */
    rt_timer_init(&fms_timer, "fms",
                    fms_time_update,
                    RT_NULL,
                    rt_tick_from_millisecond(1),
                    RT_TIMER_FLAG_PERIODIC | RT_TIMER_FLAG_HARD_TIMER);
    rt_timer_start(&fms_timer);

    while(1)
    {
        res = rt_event_recv(&fms_event, wait_set, RT_EVENT_FLAG_OR | RT_EVENT_FLAG_CLEAR, 
                                RT_WAITING_FOREVER, &recv_set);
        
        if(res == RT_EOK){
            if(recv_set & FMS_EVENT_LOOP){
                fms_loop();
            }
        }
    }
}

gp_err task_fms_init(void)
{
    rt_err_t res;

    fms_ctor();

    res = rt_thread_init(&thread_fms_handle,
                           "fms",
                           fms_entry,
                           RT_NULL,
                           &thread_fms_stack[0],
                           sizeof(thread_fms_stack),PRIORITY_FMS,5);

    RT_ASSERT(res == RT_EOK);

    if (res == RT_EOK) {
        rt_thread_startup(&thread_fms_handle);
    }

    brd_set_vehicle_init_stage(INIT_STAGE_FMS);

    return res;
}
/*------------------------------------test------------------------------------*/


